#include "elite_robot_sdk.h"
#include "logger.h"

EliteRobotSdk::EliteRobotSdk()
{

}

bool EliteRobotSdk::init()
{
    sync_client.init(mPort, mIP.c_str());
    if(!sync_client.connect())
    {
        LOG(ERROR) << "client connect to server failed!!!\n";
        return false;
    }
    return true;
}

bool EliteRobotSdk::isConnected()
{
    return sync_client.isConnected();
}

bool EliteRobotSdk::disconnect()
{
    sync_client.disconnect();
    return true;
}

int EliteRobotSdk::getServoStatus(bool& reslut)
{
    std::string cmd = protocol.sdk_assemble_getServoStatus_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }
    return deal_bool_reply(reslut);
}

int EliteRobotSdk::set_servo_status(ELITE_SDK_SWTICH servo_enable,bool& reslut)
{
    if(ELITE_SDK_DISABLE!=servo_enable && ELITE_SDK_ENABLE!=servo_enable){
        LOG(ERROR) << "switch_enable = " << servo_enable << " out of range"<< std::endl;
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteSetServoStatus  servo_status;
    servo_status.status = servo_enable;
    std::string cmd = protocol.sdk_assemble_set_servo_status_str(servo_status);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }
    return deal_bool_reply(reslut);
}

int EliteRobotSdk::setCollisionEnable(ELITE_SDK_SWTICH switch_enable,bool& reslut)
{
    EliteColEnable col_enable;
    if(ELITE_SDK_DISABLE!=switch_enable && ELITE_SDK_ENABLE!=switch_enable){
        LOG(ERROR) << "switch_enable = " << switch_enable << " out of range"<< std::endl;
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    col_enable.enable = switch_enable;
    std::string cmd = protocol.sdk_assemble_setCollisionEnable_str(col_enable);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::stop(bool &reslut)
{
    std::string cmd = protocol.sdk_assemble_stop_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::setSpeed(double speed_value, bool &reslut)
{
    if(speed_value < 0.05 || speed_value > 100){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteSpeedValue elite_speed_value;
    elite_speed_value.value = speed_value;
    std::string cmd = protocol.sdk_assemble_setSpeed_str(elite_speed_value);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::runJbi(std::string filename, bool& reslut)
{
    EliteJbiFilename elite_filename;
    if(filename == ""){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    elite_filename.filename = filename;
    std::string cmd = protocol.sdk_assemble_runJbi_str(elite_filename);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set collision sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::getSysVarI(int addr, int &value)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteGetSystemVar varI;
    varI.addr = addr;
    std::string cmd = protocol.sdk_assemble_getSysVarI_str(varI);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(), cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "get SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_int_reply(value);
}

int EliteRobotSdk::setSysVarI(int addr, int value, bool& reslut)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    if(value < -32767 || value > 32767){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    EliteSetSystemVar<int> varI;
    varI.addr = addr;
    varI.value = value;
    std::string cmd = protocol.sdk_assemble_setSysVarI_str(varI);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::getSysVarD(int addr, double &value)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteGetSystemVar varD;
    varD.addr = addr;
    std::string cmd = protocol.sdk_assemble_getSysVarD_str(varD);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(), cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "get SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_double_reply(value);
}

int EliteRobotSdk::setSysVarD(int addr, double value, bool &reslut)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    if(value < -1e+09 || value > 1e+09){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    EliteSetSystemVar<double> varD;
    varD.addr = addr;
    varD.value = value;
    std::string cmd = protocol.sdk_assemble_setSysVarD_str(varD);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::getSysVarB(int addr, int &value)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteGetSystemVar varB;
    varB.addr = addr;
    std::string cmd = protocol.sdk_assemble_getSysVarB_str(varB);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(), cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "get SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_int_reply(value);
}

int EliteRobotSdk::setSysVarB(int addr, int value, bool &reslut)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    if(value <0){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteSetSystemVar<int> varB;
    varB.addr = addr;
    varB.value = value;
    std::string cmd = protocol.sdk_assemble_setSysVarB_str(varB);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::getSysVarV(int addr, Param6D &value)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }

    EliteGetSystemVar varV;
    varV.addr = addr;
    std::string cmd = protocol.sdk_assemble_getSysVarB_str(varV);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(), cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "get SysVarI sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_6d_reply(value);
}

int EliteRobotSdk::setSysVarV(int addr, Param6D value, bool &reslut)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    EliteSetSystemVar<Param6D> varV;
    varV.addr = addr;
    double *tmp = (double *)&varV.value;
    for(int i=0;i<6;i++){
        tmp[i] = value[i];
    }

    std::string cmd = protocol.sdk_assemble_setSysVarV_str(varV);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set SysVarV sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::setSysVarP(int addr, Param6D value, bool &reslut)
{
    if(addr < 0 || addr > 255){
        return -ELITE_SDK_SYNC_PARAMS_ERR;
    }
    EliteSetSystemVar<Param6D> varP;
    varP.addr = addr;
    double *tmp = (double *)&varP.value;
    for(int i=0;i<6;i++){
        tmp[i] = value[i];
    }

    std::string cmd = protocol.sdk_assemble_setSysVarP_str(varP);
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set  SysVarP sync_send_data err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::syncMotorStatus(bool &reslut)
{
    std::string cmd = protocol.sdk_assemble_syncMotorStatus_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "syncMotorStatus err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::getMotorStatus(bool &reslut)
{
    std::string cmd = protocol.sdk_assemble_getMotorStatus_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "getMotorStatus  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::clearAlarm(bool &reslut)
{
    std::string cmd = protocol.sdk_assemble_clearAlarm_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set clearAlarm  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
}

int EliteRobotSdk::get_joint_pos(Param6D &reslut)
{
    std::string cmd = protocol.sdk_assemble_get_joint_pos_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "get joint pos err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_6d_reply(reslut);
}

int EliteRobotSdk::get_tcp_pose(Param6D &reslut)
{
    std::string cmd = protocol.sdk_assemble_get_tcp_pose_str();
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR) << "set clearAlarm  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_6d_reply(reslut);
}

int EliteRobotSdk::mov_joint_pos(EliteMoveByjoint move_by_joint, bool &reslut)
{
    std::string cmd = protocol.sdk_assemble_move_by_joint_str(move_by_joint);
#if 1
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "set clearAlarm  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    return deal_bool_reply(reslut);
#else
    LOG(INFO) << "cmd =" << cmd;
    return 0;
#endif
}

int EliteRobotSdk::mov_tcp_pose(EliteInverseKinematic inverse_kinematic, bool &reslut)
{
    //逆解+关节运动
    std::string cmd = protocol.sdk_assemble_invertKinematic_str(inverse_kinematic);
#if 1
    int nRet = sync_client.sync_send_data((char*)cmd.c_str(),cmd.length(), 2000);
    if(-1 == nRet){
        LOG(ERROR) << "set clearAlarm  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    Param6D joint_pos;
    EliteMoveByjoint move_by_joint;
    if(ELITE_SDK_PARSE_JSON_OK != deal_6d_reply(joint_pos)){
        LOG(ERROR) << "deal_6d_reply  err!!!\n";
        return -ELITE_SDK_SYNC_SEND_DATA_ERR;
    }

    move_by_joint.speed = 40;
    memcpy(move_by_joint.joint_pos,joint_pos, sizeof(Param6D));
    return mov_joint_pos(move_by_joint, reslut);
#else
    LOG(INFO) << "cmd =" << cmd;
    return 0;
#endif

}


int EliteRobotSdk::deal_bool_reply(bool& reslut)
{
    int nReslut;
    std::string jsonStr(sync_client.mData,sync_client.mDataLen);
    int nRet =  protocol.sdk_prase_bool_reply(jsonStr,nReslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "prase bool reply err!!!\n";
        return nRet;
    }

    if(ELITE_SDK_PARSE_FALSE == nReslut){
        reslut = false;
    }else{
        reslut = true;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdk::deal_int_reply(int &reslut)
{
    std::string jsonStr(sync_client.mData,sync_client.mDataLen);
    int nRet =  protocol.sdk_prase_int_reply(jsonStr, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "prase bool reply err!!!\n";
        return nRet;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdk::deal_double_reply(double &reslut)
{
    std::string jsonStr(sync_client.mData,sync_client.mDataLen);
    int nRet =  protocol.sdk_prase_double_reply(jsonStr, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "prase bool reply err!!!\n";
        return nRet;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdk::deal_6d_reply(Param6D &reslut)
{
    std::string jsonStr(sync_client.mData, sync_client.mDataLen);
    int nRet =  protocol.sdk_prase_6d_reply(jsonStr, reslut);
    if(ELITE_SDK_PARSE_JSON_OK != nRet){
        LOG(ERROR) << "prase bool reply err!!!\n";
        return nRet;
    }
    return ELITE_SDK_PARSE_JSON_OK;
}
